/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#ifndef _CCornerMapMCL__H_
#define _CCornerMapMCL__H_

#include "CParticleFilter.h"

#define VMAP 0.2					// Map uncertainty
#define VANG 0.2					// Angle uncertainty
#define VR	 0.02					// Uncertainty of the range measurement
#define FALSE_PROB 0.3				// "Probability" of the false measurement

#define VMEAS ((VMAP+VANG+VR)*(VMAP+VANG+VR)) // Whole measurement uncertainty

#define SPLIT_AND_MERGE_D 6 ///<dimension of the split and merge return (x1,y1,x2,y2,startInd,stopInd)
#define MAX_POINT_DIST 0.6 ///<something to do with merging
class CCornerExtractor{
		public: 
				FILE *out;
				FILE *corners;
				CCornerExtractor(){
						
				}
				~CCornerExtractor(){
						fclose(out);
						fclose(corners);
				}
				
/**
	* 
	* @param x x-coordinates of points to be segmented    
	* @param y y-coordinates of points to be segmented
	* @param line output i.e. each column specifies one 
  *        segmented line with starting point, endpoint 
	*        and indexes of the starting and end point.
	* @param nump number of lines found
	* @param size size of x and y arrays.
	* @param Th Treshold distance from point to line.
*/
void split_and_merge(float *x, float *y, float *line, int *nump, int size, float Th){
		int i,j,k,end,ind,split,splitlist[1024],numtotest,splits[512];
		float a,b,c,norm,dx,dy,dist, meanDist;
		int start;
		int stop;
		int endOfLine;

		start=0;
		stop = size;

		/* split */
		splitlist[0]=stop-1;
		numtotest=1;
		j=1;
		splits[0]=start;
		while(numtotest>0)
		{
				end=splitlist[numtotest-1];
				if(end < 0 || start < 0) {
						(*nump)=0;
						return;
				}	
				/* test split */
				/* calculate line parameters from start and end points of the segment */
				dx=x[start]-x[end];
				dy=y[start]-y[end];
				norm=sqrt(dx*dx+dy*dy);
				a=dy/norm;
				b=-dx/norm;
				c=-a*x[start]-b*y[start];
		
				/* check if the points between the two endpoints are close enough to the line*/ 
				split=0;
				for(i=start;i<end;i++)
				{
		
						dist=a*x[i]+b*y[i]+c;
						if(fabs(dist)>Th) /* if not then new split in the middle of this segment */
						{
								splitlist[numtotest]=(start+end)/2;
								numtotest++;
								split=1;
								break;
						}  
				}
				if(split==0) /* else test next segment */
				{
						splits[j]=end;
						j++;
						numtotest--;
						start=end;
				}
		}
		
		/* merge */
		start=splits[0];
		k=0;
		
		for(i=2;i<j;i++)
		{
				end=splits[i];
				ind=splits[i-1];
				dx=x[start]-x[end];
				dy=y[start]-y[end];
				norm=sqrt(dx*dx+dy*dy);
				a=dy/norm;
				b=-dx/norm;
				c=-a*x[start]-b*y[start];
				dist=a*x[ind]+b*y[ind]+c;
				if(fabs(dist)>Th)
				{
						line[k*SPLIT_AND_MERGE_D]=x[start];      //start
						line[k*SPLIT_AND_MERGE_D+1]=y[start];
						line[k*SPLIT_AND_MERGE_D+2]=x[ind];    //ind     
						line[k*SPLIT_AND_MERGE_D+3]=y[ind];
						line[k*SPLIT_AND_MERGE_D+4]=start;
						line[k*SPLIT_AND_MERGE_D+5]=ind;
						k++;
						start=ind;
				}
		}
		line[k*SPLIT_AND_MERGE_D]=x[start];
		line[k*SPLIT_AND_MERGE_D+1]=y[start];
		line[k*SPLIT_AND_MERGE_D+2]=x[end];
		line[k*SPLIT_AND_MERGE_D+3]=y[end];
		line[k*SPLIT_AND_MERGE_D+4]=start;
		line[k*SPLIT_AND_MERGE_D+5]=end;
		k++;
		
		(*nump)=k;


				// Check extracted lines
		for(i=0;i<k;i++){
				start = (int)line[i*SPLIT_AND_MERGE_D+4];
				meanDist=0;
				// Calculate average distance between points
				for(j=(int)start;j<=(int)line[i*SPLIT_AND_MERGE_D+5];j++){	 
						meanDist += sqrt( (x[j]-x[j+1])*(x[j]-x[j+1]) + (y[j]-y[j+1])*(y[j]-y[j+1]));
				}	
				meanDist = meanDist/(line[i*SPLIT_AND_MERGE_D+5]-line[i*SPLIT_AND_MERGE_D+4]);

				endOfLine = (int)line[i*SPLIT_AND_MERGE_D+5];

				for(j=(int)start;j<=endOfLine;j++){
						//distance between neighbour points 
						dist = sqrt( (x[j]-x[j+1])*(x[j]-x[j+1]) + (y[j]-y[j+1])*(y[j]-y[j+1]));
						// if too large reject
						if(dist> 2.0 * meanDist || dist>MAX_POINT_DIST){
								if( (j-(int)line[i*SPLIT_AND_MERGE_D+4]) > ((int)line[i*SPLIT_AND_MERGE_D+5]-j) ){
										line[i*SPLIT_AND_MERGE_D+5] = j;
										j=endOfLine+1;
								}
								else{
										line[i*SPLIT_AND_MERGE_D+4]=j+1;
								}
						}
				}
		}

		(*nump)=k;
}

		void filterScan(mcl::scan &scan){
				int i;
				float dl;
				///Try to filter out the error values
				for (i=0;i<scan.N;i++){
						if(scan.r[i]>80){
								if(i==0){
										int j=0;
										while(scan.r[j]>80 && j<scan.N) j++;
										scan.r[i]=scan.r[j];
								}
								else if(i=(scan.N-1)){
										scan.r[i]=scan.r[i-1];
								}
								else{
										scan.r[i] = (scan.r[i-1]+scan.r[i+1])*0.5;
								}
						}
				}
				//some sort of filtering 
				for(i=1;i<(scan.N-2);i++){
						dl=fabs(scan.r[i+1] - scan.r[i]);
						if(dl>0.5){
								scan.r[i+1] = (scan.r[i] + scan.r[i+2])*0.5;
						}
				}
		}
		void extractScan(mcl::scan &scan){
				float *x,*y;
				float *lines,*filtLines;
				int numOfLines,num=0;
				bool cornerCandidate=false;
				static int cnt=0;
				
				filterScan(scan);
				x = (float *) malloc(scan.N*sizeof(float));
				y = (float *) malloc(scan.N*sizeof(float));
				lines = (float *) malloc(scan.N*sizeof(float)* SPLIT_AND_MERGE_D);
				filtLines = (float *) malloc(scan.N*sizeof(float)* SPLIT_AND_MERGE_D);
				
				for(int i=0;i<scan.N;i++){
						x[i] = (float)scan.r[i] * cos(scan.a[i]);
						y[i] = (float)scan.r[i] * sin(scan.a[i]);
				}
				split_and_merge(x, y, lines, &numOfLines, scan.N, 1.0);
				fprintf(stderr,"Found %d lines...",numOfLines);
				out = fopen("lines.txt","wt");
				corners = fopen("corners.txt","wt");
				float l;
				
				//filter the lines
				for(int i=0;i<numOfLines;i++){
						float x1 = lines[SPLIT_AND_MERGE_D*i];
						float y1 = lines[SPLIT_AND_MERGE_D*i+1];
						float x2 = lines[SPLIT_AND_MERGE_D*i+2];
						float y2 = lines[SPLIT_AND_MERGE_D*i+3];
						
						l = sqrt((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2));
						float start = lines[SPLIT_AND_MERGE_D*i+4];
						float end = lines[SPLIT_AND_MERGE_D*i+5];
						if((end-start)>10 && l<30){ ///< don't accepct too long lines or lines with too few points
								filtLines[num*SPLIT_AND_MERGE_D]= x1;
								filtLines[num*SPLIT_AND_MERGE_D+1]= y1;
								filtLines[num*SPLIT_AND_MERGE_D+2]= x2;
								filtLines[num*SPLIT_AND_MERGE_D+3]= y2;
								filtLines[num*SPLIT_AND_MERGE_D+4]= start;
								filtLines[num*SPLIT_AND_MERGE_D+5]= end;
								num++;
						}
				}
				fprintf(stderr,"after filter there is %d lines left\n",num);
				for(int i=0;i<num;i++){
						fprintf(out,"%.3f %.3f %.3f %.3f\n",
																		lines[SPLIT_AND_MERGE_D*i],
																		lines[SPLIT_AND_MERGE_D*i+1],
																		lines[SPLIT_AND_MERGE_D*i+2],
																		lines[SPLIT_AND_MERGE_D*i+3]);
						
						
						float x1 = lines[SPLIT_AND_MERGE_D*i];
						float y1 = lines[SPLIT_AND_MERGE_D*i+1];
						float x2 = lines[SPLIT_AND_MERGE_D*i+2];
						float y2 = lines[SPLIT_AND_MERGE_D*i+3];
						l = sqrt((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2));
						
						if(l>3.0){
								if(!cornerCandidate){
										cornerCandidate = true;	
								}
								else{///<lets call this a corner
										
										fprintf(corners,"%.3f %.3f\n",x1,y1);
								}
						}
						else{
								cornerCandidate = false;
						}
				}	
				fclose(out);
				fclose(corners);
				if(num>10){
						cnt++;
						fprintf(stderr,"In total %d failures\n",cnt);
				exit(1);
				}
				free(x);
				free(y);
				free(lines);
				free(filtLines);
		}
										  
		private:
};

class CCornerMapMCL{
		public:
				CParticleFilter filt; ///<the particle filter used
				CCornerMapMCL();
				~CCornerMapMCL();
				/**
				 * Initializes the filter - uses normal random method for calculating the initial distribution
				 * Loads the map from file, reserves memory for particles, sets the initial distribution
				 * @p *mapfile The mapfile name
				 * @p mapscale the scale factor needed to get the map coordinates into meters
				 * @p num_of_particles The number of particles in the filter
				 * @p init_pose the initial pose
				 * @p initVariance the variance for the initial quess
				 * @p num_of_meas_in_scan number of measurements used from the scan
				 */
				void initialize(const char *mapfile,
														float mapscale, 
														int num_of_particles, 
														mcl::pose initPose, 
														mcl::pose initVariance,
														int num_of_meas_in_scan,
														float _sensor_offset);

				/**
				* Same as init but only for the filter
				* Initializes the filter - uses normal random method for calculating the initial distribution
				*/
				void reinitializeNR(mcl::pose initPose,mcl::pose initVariance);
				
				/**
				* Run the MCL step
				* @p dP the estimated relative movement between steps (prediction)
				* @p variance the error of prediction
				* @p measurement the scan taken from new position
				*/
				void runMCL(mcl::pose dP, mcl::pose variance, mcl::scan measurement);
				
		private:
				int num_of_meas;
				float sensor_offset;
				//TLineMap map;
				
				/**
				* Calculates the likelihood of particle distribution
				* uses the CParticleFilter filt and TLineMap map
				* @param meas The current measurement
				**/
				void calculateLikelihood(mcl::scan meas);
};

#endif


